#ifndef CHASSIS_H

/**
 *  @brief �˴�Ϊ����ģ�� �����ﵽ����ϵ�Ч�� �ڴ˴����е�ȫ�ֱ�����ʹ�þֲ������Ĵ��� ��task��ʵ�ֽṹ���ʵ����
 *  Ŀǰ���̰���������Ҫ�Ĵ�ṹ��
 * 
 *   @param(out)  ChassisImu_t
 *   @param(out)  ops_t
 *   @param(out)  Chassis_t
 * 
 *  @brief  �����̰����ܻ���Ϊ��ͬ�Ľṹ�� �������ƣ�
 *          λ�˼��
 *          ���Ʒ�ʽ��ģʽ������
 *          һЩ�̶�����(�������࣬���ѡ�� pid����������)
 *          �������˶�����
 *          ����ķ���ֵ ������� ���������ֵ
 *          ���еĽṹ�嶼������Chassis_t �������жԵ���״̬���޸� 
 * 
 *  @attention  ����pid�Ĳ�������������config�ļ��н����޸� 
 *  ��Ҫ�ⲿ���õĺ��������������״̬���ж� �����������̵߳������������
 *  
 * 
 * 
 * 
*/

#define CHASSIS_H

#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "map.h"
#include "user_math.h"
#include "ahrs.h"
#include "can_use.h"
#include "cmd.h"
#include "filter.h"

#define CHASSIS_OK (0)      
#define CHASSIS_ERR (-1)     
#define CHASSIS_ERR_NULL (-2) 
#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */

//m3508的电机转速转换为底盘的实际速度
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f  


#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f 
#define PI 3.1415926535f

typedef struct {

 BMI088_t bmi088;

 /*可通过该枚举类型来决定Imu的数据量纲*/
 enum {
  IMU_DEGREE,//角度值（0-360）
  IMU_RADIAN//弧度制（0-2pi)
 }angle_mode;

 AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}ChassisImu_t;

/*底盘的类型*/
typedef enum {
  CHASSIS_TYPE_MECANUM,    /* 麦轮 */
  CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
  CHASSIS_TYPE_AGV,		/* AGV舵轮 */
} Chassis_Type_e;

/*底盘的电机轮组*/
typedef enum {
     DJI_M3508,
     DJI_G6020,
	   AGV_Group,
}Chassis_Motortype_e;


/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct
{
  Chassis_Type_e chassis_type; /*  */
  Chassis_Motortype_e motor_type; /**/
	
	
	/*该部分决定PID的参数整定在config中修改*/
  pid_param_t M3508_param;	
  pid_param_t AngleCor_param;
  pid_param_t OmegaCor_param; 
  pid_param_t DisCamera_param;    
  pid_param_t ImuCor_param;
  pid_param_t C6020pitAngle_param;
  pid_param_t C6020pitOmega_param;	
  pid_param_t Gimbal_yawAngle_param;
  pid_param_t Gimbal_yawOmega_param;	
  pid_param_t Gimbal_pitchAngle_param;
  pid_param_t Gimbal_pitchOmega_param;	
  pid_param_t NaviVx_param;
  pid_param_t NaviVy_param;
  pid_param_t NaviVw_param;	
	pid_param_t Sick_CaliYparam;
	pid_param_t Sick_CaliXparam;
}Chassis_Param_t;
/*该结构体用于底盘的期望运动向量*/
typedef struct 
{
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;
	fp32 mul;//油门倍率
}ChassisMove_Vec;


/**
 *  @brief  
 * 
 */
typedef struct{
	
	  uint8_t chassis_task_run;                  //线程的运行

   const	Chassis_Param_t *param;        //一些固定的参数

    ChassisImu_t pos088;                    //088的实时姿态

    CMD_Chassis_CtrlType_e ctrl;            
	  CMD_Chassis_mode_e mode;                 

    ChassisMove_Vec move_vec;         //由控制任务决定

    struct{
			
	  fp32 rotor_rpm3508[4];
	  fp32 rotor_current3508[4];
			
	  fp32 rotor_pit6020angle;
	  fp32 rotor_pit6020rpm;

	  fp32 rotor_gimbal_yawangle;
	  fp32 rotor_gimbal_yawrpm;

	  fp32 rotor_gimbal_pitchangle;
	  fp32 rotor_gimbal_pitchrpm;

	}motorfeedback;
    

	/*期望的底盘输出值(此处为舵轮解算出的各个电机的期望输出值)ֵ*/
	struct{
	fp32 OmniSpeedOut[4];
	fp32 pit6020_target;
	fp32 gimbal_yawtarget;
	fp32 gimbal_pitchtarget;

	}hopemotorout;

	fp32 pit6020_fback;
	fp32 gimbal_yaw_fback;
	fp32 gimbal_pitch_fback;

	/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
	{
		fp32 final_3508out[4];
		fp32 final_pitchout;
		fp32 final_gimbal_yawout;
		fp32 final_gimbal_pitchout;

	}final_out;
	
	struct{                
	pid_type_def chassis_AngleCorPID;                         
	pid_type_def chassis_OmegaCorPID;  
	pid_type_def chassis_DisCameraPID;  
    pid_type_def chassis_ImuCorPID;		
	pid_type_def chassis_3508VecPID[4];
	pid_type_def chassis_pitAngle6020;
	pid_type_def chassis_pitOmega6020;
	pid_type_def chassis_gimbal_yawAnglePID;
	pid_type_def chassis_gimbal_yawOmegaPID;
	pid_type_def chassis_gimbal_pitchAnglePID;
	pid_type_def chassis_gimbal_pitchOmegaPID;
	pid_type_def chassis_NaviVxPID;
	pid_type_def chassis_NaviVyPID;
	pid_type_def chassis_NaviWzPID;
	pid_type_def sick_CaliforYPID;
	pid_type_def sick_CaliforXPID;
	}pid;

   fp32 vofa_send[8];
	
	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
	 
	uint8_t  pos_byimu; //上下坡检测 1 是在上面 2是在下面
	
	int32_t sick_dis[3];   //获取到的sick激光值
	
}Chassis_t;

/**
 * @brief 
 *
 * @param c 
 * @param param 
 * @param mech_zero 
 * @param wheelPolar 
 * @return 
 */
 int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);


/**
 * \brief 

 */
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);


/**
 * \brief 

 */
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);

/// @brief 
/// @param c 
void vesc_current_detection(Chassis_t *c);
#endif
